package com.yuneec.droneservice.command;

import com.yuneec.droneservice.model.ControllerLocation;
import com.yuneec.droneservice.request.IResponse;
import com.yuneec.droneservice.utils.ByteUtils;

/**
 * Created by gjw on 2017/3/27.
 */

public class SetControllerGPS extends BaseCommand {
    /**
     * @param usePhoneGPS 是否使用手机gps，如果使用手机gps则所传gps数值将替换遥控器gps，如果不用手机gps则使用遥控器gps
     * @param location    手机gps数值
     */
    public SetControllerGPS(boolean usePhoneGPS, ControllerLocation location) {
        super(MAV_RC_COMMAND.MAV_RC_CMD_SET_GPS);
        data = new byte[18];
        data[0] = (byte) (usePhoneGPS ? 1 : 0);
        byte[] tmp = ByteUtils.intToByteArray((int) (location.longitude * 1e7), IS_BIGENDIAN);
        System.arraycopy(tmp, 0, data, 1, 4);
        tmp = ByteUtils.intToByteArray((int) (location.latitude * 1e7), IS_BIGENDIAN);
        System.arraycopy(tmp, 0, data, 5, 4);
        tmp = ByteUtils.shortToByteArray((short) (location.altitude * 100), IS_BIGENDIAN);
        System.arraycopy(tmp, 0, data, 9, 2);
        data[11] = (byte) location.satelliteCount;
        tmp = ByteUtils.shortToByteArray((short) location.accuracy, IS_BIGENDIAN);
        System.arraycopy(tmp, 0, data, 12, 2);
        tmp = ByteUtils.shortToByteArray((short) location.speed, IS_BIGENDIAN);
        System.arraycopy(tmp, 0, data, 14, 2);
        tmp = ByteUtils.shortToByteArray((short) location.angle, IS_BIGENDIAN);
        System.arraycopy(tmp, 0, data, 16, 2);
    }


    @Override
    public IResponse toResponse(byte[] data, int off, int len) {
        int code = data[off + 1] & 0xff;
        return new IResponse(ids(), code);
    }
}
